#include "bsp_usart.h"
#include <stdio.h>
#include <stdarg.h>
#include "Delay.h"
#include "bsp_led.h"
#include <string.h>
#include "pid.h"
#include "bsp_oled.h"

// 串口状态机
USARTStateMachine_t USARTStater; // 用于串口接收0xFE 或 @开头的数据包
uint8_t Serial_RxBuff = 0xff;     // 用于串口接收一个字节数据的缓冲区

/**
 * @brief 串口参数配置
 * 
 * @param Baud 波特率
 */
void USART_Config(uint32_t Baud)
{
    /* 开启时钟 */
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART1, ENABLE);
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);

    /* 配置引脚复用 */
    GPIO_PinAFConfig(USART_GPIO, GPIO_PinSource9, GPIO_AF_USART1);
    GPIO_PinAFConfig(USART_GPIO, GPIO_PinSource10, GPIO_AF_USART1);

    /* 初始化GPIO */
    GPIO_InitTypeDef GPIO_InitStruct    = {0};
    GPIO_InitStruct.GPIO_Pin            = USART_TX_Pin | USART_RX_Pin;    // 发送
    GPIO_InitStruct.GPIO_Mode           = GPIO_Mode_AF;
    GPIO_InitStruct.GPIO_Speed          = GPIO_High_Speed;
    GPIO_InitStruct.GPIO_OType          = GPIO_OType_PP;
    GPIO_InitStruct.GPIO_PuPd           = GPIO_PuPd_UP;
    GPIO_Init(USART_GPIO, &GPIO_InitStruct);

    /* 初始化USART参数 */
    USART_InitTypeDef USART_InitStruct = {0};
    USART_InitStruct.USART_BaudRate            = Baud;
    USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
    USART_InitStruct.USART_Mode                = USART_Mode_Tx | USART_Mode_Rx;
    USART_InitStruct.USART_Parity              = USART_Parity_No;
    USART_InitStruct.USART_StopBits            = USART_StopBits_1;
    USART_InitStruct.USART_WordLength          = USART_WordLength_8b;
    USART_Init(Serial, &USART_InitStruct);

    /* 配置USART中断 */
    USART_ClearFlag(Serial, USART_FLAG_TC);
    USART_ITConfig(Serial, USART_IT_RXNE, ENABLE);

    /* 初始化NVIC */
    NVIC_InitTypeDef NVIC_InitStruct = {0};
    NVIC_InitStruct.NVIC_IRQChannel                   = Serial_IRQ;
    NVIC_InitStruct.NVIC_IRQChannelCmd                = ENABLE;
    NVIC_InitStruct.NVIC_IRQChannelPreemptionPriority = 1;
    NVIC_InitStruct.NVIC_IRQChannelSubPriority        = 1;
    NVIC_Init(&NVIC_InitStruct);

    /* 开启USART */
    USART_Cmd(Serial, ENABLE);
}

/**
 * @brief 串口发送一个字节的数据
 * 
 * @param Byte 
 */
void Serial_SendByte(uint8_t Byte)
{
    uint32_t timeOut = 10000;

    USART_SendData(Serial, Byte);  // 发送后等待发送完成
    while (USART_GetFlagStatus(Serial, USART_FLAG_TXE) != SET) {
        timeOut--;
        if (timeOut == 0) break;
    }
}

/**
 * @brief 串口发送一个数组的数据
 * 
 * @param Array 数组首地址
 * @param Length 数组长度
 */
void Serial_SendArray(uint8_t *Array, uint16_t Length)
{
    uint16_t i = 0;
    for (i = 0; i < Length; i++) {
        Serial_SendByte(Array[i]);
    }
}

/**
 * @brief 串口发送字符串
 * 
 * @param String 字符串首地址
 */
void Serial_SendString(char *String)
{
    uint8_t i = 0;
    for (i = 0; String[i] != '\0'; i++) {
        Serial_SendByte(String[i]);
    }
}

/**
 * @brief 计算x的y次方
 * 
 * @param x 
 * @param y 
 * @return uint32_t 
 */
static uint32_t Serial_Pow(uint32_t x, uint32_t y)
{
    uint32_t Result = 1;
    while (y--) {
        Result *= x;
    }
    return Result;
}

static float Serial_PowInvert(uint8_t X, uint8_t n)//x除以n次10
{
  float result=X;
	while(n--)
	{
		result/=10;
	}
	return result;
}

/**
 * @brief 串口发送十进制数字
 * 
 * @param Num 十进制数字
 * @param Length 长度
 */
void Serial_SendNum(uint32_t Num, uint8_t Length)
{
    uint8_t i = 0;
    for (i = 0; i < Length; i++) {
        Serial_SendByte(Num / Serial_Pow(10, Length - i - 1) % 10 + '0'); // 每次发送十进制的一位
    }
}

/**
 * @brief USART轮询模式
 * 重定向c库函数printf到串口，以使用printf
 * 
 * @param ch 
 * @param f 
 * @return int 
 */
int fputc(int ch, FILE *f)
{
    /* 发送一个字节数据到串口 */
    Serial_SendByte(ch);	
    
    return (ch);
}

/**
 * @brief USART轮询模式
 * 重定向c库函数scanf到串口，以使用scanf、getchar
 * 
 * @param f 
 * @return int 
 */
int fgetc(FILE *f)
{		
    int ch;
    ch = USART_ReceiveData(Serial);	
    return (ch);
}

/**
 * @brief 串口封装sprintf
 * 
 * @param format 
 * @param ... 
 */
void Serial_Printf(char *format, ...)
{
    char string[100];
    va_list arg;
    va_start(arg, format);
    vsprintf(string, format, arg);
    va_end(arg);
    Serial_SendString(string);
}

/**
 * @brief 从串口接收到的pid数据包中提取pid参数，str格式为 Kp=%.2f
 * 
 * @param str 串口接收到的字符串
 * @param pData 要保存的pid参数的变量地址
 * @return uint8_t 返回 0 -- 该参数是Kp
 *                      1 -- 该参数是Ki
 *                      2 -- 该参数是Kd
 */
uint8_t Serial_PidHandle(char* str, float *pData)//数据包换算处理
{
    float Data = 0.0;
    uint8_t dot_Flag = 0;//小数点标志位，能区分小数点后或小数点前 0为小数点前，1为小数点后
    uint8_t dot_after_num = 1;//小数点后的第几位
    int8_t minus_Flag = 1;// 负号标志位 -1为是负号 1为正号

    /* 确定要修改的变量：Kp、Ki、Kd其中之一 */
    char var[3];
    var[0] = str[0];
    var[1] = str[1];
    var[2] = '\0';
    // printf("%s\n", str);

    /* 字符串转为数字 */
    char *pNum = &str[3];
    uint16_t data_unm = strlen(pNum);
    for (uint8_t i=0; i<data_unm; i++)
    {
        if (pNum[i] == 0x2D)//如果第一位为负号
        {
            minus_Flag=-1;
            continue;//跳过本次循环 
        }
        if (dot_Flag==0)
        {
            if (pNum[i] == 0x2E)//如果识别到小数点，则将dot_Flag置1
            {
                dot_Flag = 1;
            }
            else//还没遇到小数点前的运算
            {
                Data = Data*10 + pNum[i]-48;
            }
        }
        else//遇到小数点后的运算
        {
            Data = Data + Serial_PowInvert(pNum[i]-48, dot_after_num);
            dot_after_num++;
        }
    }
    /* 添加符号 */
    Data *= minus_Flag;
    *pData = Data;
    // printf("%.2f\n", *pData);

    /* 判断修改的是哪个pid参数 */
    if (strcmp(var, "Kp") == 0) return 0;
    else if (strcmp(var, "Ki") == 0) return 1;
    else if (strcmp(var, "Kd") == 0) return 2;
    return 0xff;
}

static uint8_t pid_flag = 0;// 接收到pid调参的命令标志位

uint8_t Serial_GetPidFlag()
{
    return pid_flag;
}

void Serial_ClearPidFlag()
{
    pid_flag = 0;
}

/**
 * @brief 串口接收到以下字符串就执行对应的命令
 * --LED_B_ON    -- 打开LED_B
 * --LED_B_OFF   -- 关闭LED_B
 * @param str 串口接收到的字符串
 */
static void Serial_CmdCheck(char *str)
{
    /* 点灯命令 */
    if (strcmp(str, "LED_G_ON") == 0)       LED_G_ON;    
    else if (strcmp(str, "LED_G_OFF") == 0) LED_G_OFF;
    else if (str[0] == 'K')                 pid_flag = 1;// 数据包首字符为K，表示调pid参数
}


/**
 * @brief 串口状态检测
 * --HEX模式：接收到的第一个数据为0xFF，最后一个数据为0xFE时，接收的数据为十六进制
 * --ASCII码模式：接收到第一个数据为@，最后一个数据为回车时，接受的数据为ASCII码 
 * 
 * @param pUSARTStater 串口状态机的指针
 */
static void Serial_StateCheck(USARTStateMachine_t *pUSARTStater)
{
    /* 接收数据 */
    uint8_t RxData = USART_ReceiveData(Serial);

    /* 检测串口接收状态 */
    switch (pUSARTStater->RxState)
    {
        /* 空闲状态 */
        case USART_StateFree:
        {
            /* 收到的第一个数据为0xFF或者'@'时串口进入工作状态 */
            if (RxData == 0xFF || RxData == '@') {  
                pUSARTStater->RxState = USART_StateWork; 
                pUSARTStater->pos     = 0;

                if (RxData == 0xFF) { pUSARTStater->RxMode = USART_ModeHex;  }  // 收到0xFF，进入HEX模式
                if (RxData == '@')  { pUSARTStater->RxMode = USART_ModeASCII;}  // 收到@，进入ASCII模式
            }
            /* 不使用状态机 */
            else {
                Serial_RxBuff = USART_ReceiveData(Serial);
                // printf("不使用状态机的回显%d\n", Serial_RxBuff);
            }
            break;
        }
        /* 工作状态 */
        case USART_StateWork:
        {
            /* 清除标志位 */
            pUSARTStater->Rx_EndFlag = RESET;

            /* HEX模式 */
            if (pUSARTStater->RxMode == USART_ModeHex) {
                if (pUSARTStater->pos >= PacketHex_Size) {       // 检测到超出HEX数据包大小即进入结束状态
                    pUSARTStater->RxState = USART_StateEnd;
                }
                else {
                    pUSARTStater->Serial_RxPacket[pUSARTStater->pos] = RxData; // 保存数据到数组中
                    pUSARTStater->pos++;
                }
            }

            /* ASCII模式 */
            else if (pUSARTStater->RxMode == USART_ModeASCII) {
                if (RxData == '\r') {                          // 检测到回车即进入结束状态
                    pUSARTStater->RxState = USART_StateEnd;
                }
                else {
                    pUSARTStater->Serial_RxPacket[pUSARTStater->pos] = RxData; // 保存数据到数组中
                    pUSARTStater->pos++;
                }
            }
            break;
        }
        /* 结束状态 */
        case USART_StateEnd:
        {
            /* 清除标志位 */
            pUSARTStater->RxState      = USART_StateFree;
            pUSARTStater->Rx_EndFlag   = SET;

            /* Hex模式 */
            if (pUSARTStater->RxMode == USART_ModeHex) {
                /* 收到0xFE时表示传输结束，串口切换回空闲状态 */
                if (RxData == 0xFE) {
                    // Serial_Printf("串口已收到数据包%x\n", pUSARTStater->Serial_RxPacket);
                }
            }
            /* ASCII模式 */
            else if (pUSARTStater->RxMode == USART_ModeASCII) {
                /* 收到回车时表示传输结束，串口切换回空闲状态 */
                if (RxData == '\n') {
                    pUSARTStater->Serial_RxPacket[pUSARTStater->pos] = '\0';
                    Serial_CmdCheck(pUSARTStater->Serial_RxPacket);   // 检测命令
                    // Serial_Printf("串口已收到命令@%s\n", pUSARTStater->Serial_RxPacket);
                }
            }
            break;
        }
    }
}

/**
 * @brief 串口接收完成标志检测
 * 
 * @param pUSARTStater 串口状态机的指针
 * @return uint8_t SET or RESET
 */
uint8_t Serial_GetRxEnd(USARTStateMachine_t *pUSARTStater)
{
    return (pUSARTStater->Rx_EndFlag);
}

/* 要调的pid参数声明在这里 */
extern PID_t pidMotorA;
extern PID_t pidMotorB;
extern PID_t pidAngle;
extern float pidParam_MotorA[];
extern float pidParam_MotorB[];
extern float pidParam_Angle[];

// 发送@[Name]=%.2f\r\n改变参数
void Serial_UpdateParam()
{
    float tmp_pid = 0;

    /* 上位机PID调参 */
    if (Serial_GetPidFlag() == 1) {
        /* 调motorA的PID */
        if (Serial_PidHandle(USARTStater.Serial_RxPacket, &tmp_pid) == 0) pidParam_MotorA[0] = tmp_pid;
        else if (Serial_PidHandle(USARTStater.Serial_RxPacket, &tmp_pid) == 1) pidParam_MotorA[1] = tmp_pid;
        else if (Serial_PidHandle(USARTStater.Serial_RxPacket, &tmp_pid) == 2) pidParam_MotorA[2] = tmp_pid;
        PID_Init(&pidMotorA, DELTA_PID, pidParam_MotorA[0], pidParam_MotorA[1], pidParam_MotorA[2]);
        /* 调motorB的PID */
        if (Serial_PidHandle(USARTStater.Serial_RxPacket, &tmp_pid) == 0) pidParam_MotorB[0] = tmp_pid;
        else if (Serial_PidHandle(USARTStater.Serial_RxPacket, &tmp_pid) == 1) pidParam_MotorB[1] = tmp_pid;
        else if (Serial_PidHandle(USARTStater.Serial_RxPacket, &tmp_pid) == 2) pidParam_MotorB[2] = tmp_pid;
        PID_Init(&pidMotorB, DELTA_PID, pidParam_MotorB[0], pidParam_MotorB[1], pidParam_MotorB[2]);
        /* 调角度环的PID */
        if (Serial_PidHandle(USARTStater.Serial_RxPacket, &tmp_pid) == 0) pidParam_Angle[0] = tmp_pid;
        else if (Serial_PidHandle(USARTStater.Serial_RxPacket, &tmp_pid) == 1) pidParam_Angle[1] = tmp_pid;
        else if (Serial_PidHandle(USARTStater.Serial_RxPacket, &tmp_pid) == 2) pidParam_Angle[2] = tmp_pid;
        PID_Init(&pidAngle, POSITION_PID, pidParam_Angle[0], pidParam_Angle[1], pidParam_Angle[2]);
        /* 在OLED上显示 */
        OLED_ShowString(0, 0, "A", OLED_8X16);// 显示电机A的pid
        OLED_ShowFloatNum(8*(1), 0, pidParam_MotorA[0], 1, 2, OLED_8X16);
        OLED_ShowFloatNum(8*(1+5), 0, pidParam_MotorA[1], 1, 2, OLED_8X16);
        OLED_ShowFloatNum(8*(1+10), 0, pidParam_MotorA[2], 1, 2, OLED_8X16);
        OLED_ShowString(0, 16, "B", OLED_8X16);// 显示电机B的pid
        OLED_ShowFloatNum(8*(1), 16, pidParam_MotorB[0], 1, 2, OLED_8X16);
        OLED_ShowFloatNum(8*(1+5), 16, pidParam_MotorB[1], 1, 2, OLED_8X16);
        OLED_ShowFloatNum(8*(1+10), 16, pidParam_MotorB[2], 1, 2, OLED_8X16);
        OLED_ShowString(0, 16*2, "O", OLED_8X16);// 显示角度环的pid
        OLED_ShowFloatNum(8*(1), 16*2, pidParam_Angle[0], 1, 2, OLED_8X16);
        OLED_ShowFloatNum(8*(1+5), 16*2, pidParam_Angle[1], 1, 2, OLED_8X16);
        OLED_ShowFloatNum(8*(1+10), 16*2, pidParam_Angle[2], 1, 2, OLED_8X16);
        OLED_Update();

        // 清空标志位
        Serial_ClearPidFlag();
    }
}

/**************************************** 中断回调函数 ****************************************/

/**
 * @brief 串口中断
 * 
 */
void Serial_IRQHANDLER()
{
    if (USART_GetITStatus(Serial, USART_IT_RXNE) == SET) {
        Serial_StateCheck(&USARTStater);  // 状态检测
        USART_ClearITPendingBit(Serial, USART_IT_RXNE);
    }
}


